#include <iostream>
#include "read_data.h"

#include "optimizer.h"

using namespace std;

#if 1
int main() {  
	double acc_noise_std = 0.1;
	double init_bg = 0.0;
	std::vector<double> timestamps;
	std::vector<Eigen::Vector3d> imu_data;  
	std::vector<Eigen::Vector3d> odometry_data;
	std::vector<Eigen::Vector3d> vel_data;  
	std::vector<Eigen::Vector3d> pose_data;
	std::vector<Eigen::Vector3d> gps_data;
	read_simulate(init_bg, timestamps, imu_data, odometry_data, vel_data, pose_data, gps_data);

	Eigen::Vector2d init_ba = Eigen::Vector2d(1e-5,1e-5);
	// initial imu
	Options options;
	options.init_ba_ = init_ba;
	options.init_bg_ = init_bg;
	options.noise_acce_ = acc_noise_std;
	std::shared_ptr<ImuPreIntegration> imu_preintegration = std::make_shared<ImuPreIntegration>(options);
	
	cout << "curr init_ba ==== : " << init_ba.transpose() << std::endl;
	cout << "curr init_bg ==== : " << init_bg << std::endl;

	// initial wheel 
	WheelOptions wheel_options;
	wheel_options.sigma_x = 1e-8;
	wheel_options.sigma_y = 1e-8;
	wheel_options.sigma_t = 1e-8;
	std::shared_ptr<WheelPreIntegration> wheel_preintegration = std::make_shared<WheelPreIntegration>(wheel_options);

	// initial state
    State state;  
    memset(&state, 0.0, sizeof(state));
    std::vector<Eigen::Vector3d> pose_gt = pose_data;
    double linear_vel = 0.5;
    double true_vx = linear_vel * std::cos(pose_gt[0][2]);
    double true_vy = linear_vel * std::sin(pose_gt[0][2]);
    state.v = Eigen::Vector2d(true_vx,true_vy);
    state.ba = init_ba;
    state.bg = init_bg;

    State last_state = state; 

    Optimizer optimizer(imu_preintegration, wheel_preintegration);

    std::deque<double> timestamp_queue = convert_deque(timestamps);
    Eigen::Vector2d init_gps_pose;
    int sensor_idx = 0;

    Eigen::Vector3d last_velocity;
    Eigen::Vector3d last_pose;

    // imu_preintegration->Integrate(imu_data[0], imu_data[0], 0.1);

	for(unsigned int i = 0; i < timestamps.size()-1; i++) {

		cout << "\ncurr index ==== : " << i << std::endl;

		double delta_time = timestamps[i+1]-timestamps[i];

		// cout << "delta_time ==== : " << delta_time << std::endl;

		const Eigen::Vector3d &last_imu = imu_data[i];
		const Eigen::Vector3d &curr_imu = imu_data[i+1];

		Eigen::Vector3d last_odometry_o = odometry_data[i];
		Eigen::Vector3d curr_odometry_o = odometry_data[i+1];
		Eigen::Vector3d delta_odometry = curr_odometry_o - last_odometry_o;

		// std::cout << " last_odometry_o =======: " << last_odometry_o.transpose() << std::endl;
		// std::cout << " curr_odometry_o =======: " << curr_odometry_o.transpose() << std::endl;
		// std::cout << " delta_odometry =======: " << delta_odometry.transpose() << std::endl;

		Eigen::Vector3d last_velocity = vel_data[i];
		Eigen::Vector3d curr_velocity = vel_data[i+1];
		
		// std::cout << " last_velocity =======: " << last_velocity.transpose() << std::endl;
		// std::cout << " curr_velocity =======: " << curr_velocity.transpose() << std::endl;

		Eigen::Vector3d last_pose = pose_data[i];
		Eigen::Vector3d curr_pose = pose_data[i+1];

		SE2 last_odometry = SE2(last_odometry_o[0], last_odometry_o[1], last_odometry_o[2]);
		SE2 current_odometry = SE2(curr_odometry_o[0], curr_odometry_o[1], curr_odometry_o[2]);

		imu_preintegration->Integrate(last_imu, curr_imu, delta_time);
		wheel_preintegration->Integrate(last_odometry, current_odometry);
		
		state = imu_preintegration->Predict(last_state);

		// State& last_state_t = state;
		// wheel_preintegration->Predict(last_state_t);

		if(i > 0 && (i%5 == 0)) {

			// std::cout << " wheel dp_ =======: " << wheel_preintegration->dp_.transpose() << std::endl;
    		// std::cout << " wheel dR_ =======: " << wheel_preintegration->dR_.log() << std::endl;
    		// std::cout << " imu dp_ ==== : " << imu_preintegration->dp_.transpose() << std::endl;
    		// std::cout << " imu dv_ ==== : " << imu_preintegration->dv_.transpose() << std::endl;
    		// std::cout << " imu dR_ ==== : " << imu_preintegration->dR_.log() << std::endl;

			optimizer.OptimizeCeres(last_state, state, last_velocity, curr_velocity, last_pose, curr_pose);
			// optimizer.OptimizeLM(last_state, state, last_velocity, curr_velocity, last_pose, curr_pose);

			imu_preintegration->Clear(options);
			// imu_preintegration = std::make_shared<ImuPreIntegration>(options);
	    	wheel_preintegration = std::make_shared<WheelPreIntegration>(wheel_options);

			// cout << "curr_odometry ==== : " << curr_odometry_o.transpose() << std::endl;
			// // cout << "delta_odometry ==== : " << delta_odometry.transpose() << std::endl;
			// cout << "curr_velocity ==== : " << curr_velocity.transpose() << std::endl;
			// cout << "gt_pose ==== : " << curr_pose.transpose() << std::endl;

			// std::cout << "optimize state.R.log() ============= : " << state.R.log() << std::endl;
    		// std::cout << "optimize state.p ============= : " << state.p.transpose() << std::endl;
    		// std::cout << "optimize state.v ============= : " << state.v.transpose() << std::endl;
    		// std::cout << "optimize state.ba ============= : " << state.ba.transpose() << std::endl;
    		// std::cout << "optimize state.bg ============= : " << state.bg << std::endl;
    		// std::cout << "optimize state.cov ============= : \n" << state.cov << std::endl;
 
			last_state = state;
			usleep(150000);	

			if(i == 5) {
				exit(-1);	
			}	
		}
	}

	return 0;
}

#else 
int main() {  
	double acc_noise_std = 0.1;
	double init_bias = 0.0;
	std::vector<double> timestamps;
	std::vector<Eigen::Vector3d> imu_data;  
	std::vector<Eigen::Vector3d> odometry_data;
	std::vector<Eigen::Vector3d> vel_data;  
	std::vector<Eigen::Vector3d> pose_data;
	std::vector<Eigen::Vector3d> gps_data;
	read_simulate(init_bias, timestamps, imu_data, odometry_data, vel_data, pose_data, gps_data);

	// initial imu
	Options options;
	options.init_bg_ = init_bias;
	options.noise_acce_ = acc_noise_std;
	std::shared_ptr<ImuPreIntegration> imu_preintegration = std::make_shared<ImuPreIntegration>(options);
	
	// initial wheel 
	WheelOptions wheel_options;
	wheel_options.sigma_x = 1e-2;
	wheel_options.sigma_y = 1e-2;
	wheel_options.sigma_t = 1e-2;
	std::shared_ptr<WheelPreIntegration> wheel_preintegration = std::make_shared<WheelPreIntegration>(wheel_options);

	// initial state
    State state;  
    memset(&state, 0.0, sizeof(state));
    std::vector<Eigen::Vector3d> pose_gt = pose_data;
    double linear_vel = 0.5;
    double true_vx = linear_vel * std::cos(pose_gt[0][2]);
    double true_vy = linear_vel * std::sin(pose_gt[0][2]);
    state.v = Eigen::Vector2d(true_vx,true_vy);
    state.ba = Eigen::Vector2d(1e-6,1e-6);
    state.bg = init_bias;

    State last_state = state; 

    Optimizer optimizer(imu_preintegration, wheel_preintegration);

    std::deque<double> timestamp_queue = convert_deque(timestamps);
    Eigen::Vector2d init_gps_pose;
    int sensor_idx = 0;

    Eigen::Vector3d last_velocity;
    Eigen::Vector3d last_pose;

	for(unsigned int i = 0; i < gps_data.size()-1; i++) {
		double gps_time = gps_data[i][0];
		Eigen::Vector2d gps_pose = Eigen::Vector2d(gps_data[i][1],gps_data[i][2]);
		if(i == 0) {
			init_gps_pose = gps_pose;
			continue;
		}
		gps_pose -= init_gps_pose;

		double last_time = timestamp_queue.front();

		// cout << " last_time ==== : " << last_time << std::endl;
		Eigen::Vector3d last_imu;
		SE2 last_odometry;
		Eigen::Vector3d last_odometry_t;
		while(timestamp_queue.front() <= gps_time) {
			double time = timestamp_queue.front();
			timestamp_queue.pop_front();

			double delta_time = time-last_time;

			Eigen::Vector3d curr_imu = imu_data[sensor_idx];
			Eigen::Vector3d odometry = odometry_data[sensor_idx];
			SE2 current_odometry = SE2(odometry[0], odometry[1], odometry[2]);

			if(delta_time > 0.0000001) {
				imu_preintegration->Integrate(last_imu, curr_imu, delta_time);
				wheel_preintegration->Integrate(last_odometry, current_odometry);		
			}

			// cout << " curr_imu ==== : " << curr_imu.transpose() << std::endl;

			last_imu = curr_imu;
			last_odometry = current_odometry;
			last_time = time;
			last_odometry_t = odometry;
			sensor_idx++;
		}

		// imu_preintegration->Predict(state);

		State& last_state_t = state;
		wheel_preintegration->Predict(last_state_t);

		cout << " last_odometry_t ==== : " << last_odometry_t.transpose() << std::endl;

		// cout << " last_state_t.v ==== : " << last_state_t.v << std::endl;
		cout << " last_state_t.p ==== : " << last_state_t.p.transpose() << " , " << last_state_t.R.log() << std::endl;
		// cout << " last_state_t.R ==== : " << last_state_t.R.log() << std::endl;

		// cout << " imu_preintegration.dv_ ==== : " << imu_preintegration.dv_ << std::endl;
		// cout << " imu_preintegration.dp_ ==== : " << imu_preintegration.dp_ << std::endl;
		// cout << " imu_preintegration.dR_ ==== : " << imu_preintegration.dR_.log() << std::endl;
		// cout << " \ncurrent fusion idx is ==== : " << i << std::endl;

		Eigen::Vector3d curr_velocity = vel_data[sensor_idx];
		Eigen::Vector3d curr_pose = pose_data[sensor_idx];

		// optimizer.OptimizeLM(last_state, state, last_velocity, curr_velocity, last_pose, curr_pose);
		
		// imu_preintegration = std::make_shared<ImuPreIntegration>(options);
	    wheel_preintegration = std::make_shared<WheelPreIntegration>(wheel_options);

		cout << "last_odometry ====== : " << last_odometry.translation().transpose() << " , " << last_odometry.so2().log()  << std::endl;
		// cout << "delta_odometry ==== : " << delta_odometry.transpose() << std::endl;
		// cout << "curr_velocity ==== : " << curr_velocity.transpose() << std::endl;
		// cout << "gt_pose ==== : " << pose_data[sensor_idx].transpose() << std::endl;

		// std::cout << "optimize state.R ============= : " << state.R.log() << std::endl;
    	// std::cout << "optimize state.p ============= : " << state.p.transpose() << std::endl;
    	// std::cout << "optimize state.v ============= : " << state.v.transpose() << std::endl;
    	// std::cout << "optimize state.ba ============= : " << state.ba.transpose() << std::endl;
    	// std::cout << "optimize state.bg ============= : " << state.bg << std::endl;
    	// // std::cout << "optimize state.cov ============= : \n" << state.cov << std::endl;

		last_state = state;
		last_velocity = curr_velocity;
		last_pose = curr_pose;

		usleep(1500000);
	}

	return 0;
}

#endif
